import launch
import launch.event_handlers
import launch_ros
from ament_index_python.packages import get_package_share_directory
import launch_ros.parameter_descriptions
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    # 机器人模型名称
    robot_name_in_model = 'wheel_legged_robot'
    # 获取默认路径
    urdf_package_path = get_package_share_directory('wheel_legged_robot_description')
    default_model_path = urdf_package_path + '/urdf/wheel_legged_robot.urdf.xacro'
    default_gazebo_world_path = urdf_package_path + '/world/two_ramps.world'

    # 声明一个urdf参数方便修改
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model_path', default_value=str(default_model_path),
        description='加载的模型URDF文件的绝对路径'
    )

    # 通过文件路径获取内容，并转换成参数值对象，传递给robot_state_publihser
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(
            ['xacro ',launch.substitutions.LaunchConfiguration('model_path'),
             " ",
             "use_sim:=true",
            ]
        ),
        value_type=str
    )

    # 状态发布节点
    action_robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )

    # 通过IncludeLaunchDescription包含另外一个launch文件，来加载world环境
    action_launch_gazebo = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource([get_package_share_directory('gazebo_ros'),'/launch','/gazebo.launch.py']),
        launch_arguments=[('world',default_gazebo_world_path),('verbose','true')]
    )

    # 通过IncludeLaunchDescription包含另外一个launch文件，来加载手柄控制功能
    action_launch_joy = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource([get_package_share_directory('teleop_twist_joy'),'/launch','/teleop-launch.py']),
    )

    # gazebo加载节点
    action_gazebo_start = launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        name='wheel_legged_robot',
        arguments=['-topic','/robot_description',
                   '-entity',robot_name_in_model,
                  ],
        output="screen"
    )

    # joint_state_broadcaster节点
    joint_state_broadcaster_spawner = launch.actions.ExecuteProcess(
        cmd=['ros2','control','load_controller','--set-state','start','wheel_legged_robot_joint_state_broadcaster'],
        output='screen'
    )

    # # imu_sensor_broadcaster节点
    # imu_sensor_broadcaster_spawner = launch.actions.ExecuteProcess(
    #     cmd=['ros2','control','load_controller','--set-state','active','wheel_legged_robot_imu_sensor_broadcaster'],
    #     output='screen'
    # )

    # # 加载并激活 wheel_legged_effort_controller 控制器
    # action_load_effort_controller = launch.actions.ExecuteProcess(
    #     cmd=['ros2','control','load_controller','--set-state','active','wheel_legged_robot_effort_controller'],
    #     output='screen'
    # )

    # 加载并激活 wheel_legged_lqr_vmc_controller 控制器
    action_load_lqr_vmc_controller = launch.actions.ExecuteProcess(
        cmd=['ros2','control','load_controller','--set-state','start','wheel_legged_robot_lqr_vmc_controller'],
        output='screen'
    )

    # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node
    delay_joint_state_broadcaster_spawner_after_gazebo_spawn_robot = launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(
            target_action=action_gazebo_start,
            on_exit=[joint_state_broadcaster_spawner],
        )
    )

    # # Delay loading and activation of `imu_sensor_broadcaster` after joint_state_broadcaster
    # delay_imu_sensor_broadcaster_spawner_after_gazebo_spawn_robot = launch.actions.RegisterEventHandler(
    #     event_handler=launch.event_handlers.OnProcessExit(
    #         target_action=action_gazebo_start,
    #         on_exit=[imu_sensor_broadcaster_spawner],
    #     )
    # )

    # Delay loading and activation of `wheel_legged_lqr_vmc_controller` after joint_state_broadcaster
    delay_lqr_vmc_controller_spawner_after_joint_state_broadcaster = launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(
            target_action=joint_state_broadcaster_spawner,
            on_exit=[action_load_lqr_vmc_controller],
        )
    )

    # Delay loading and activation of `teleop_twist_joy` after wheel_legged_lqr_vmc_controller
    delay_action_launch_joy_after_lqr_vmc_controller_spawner = launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(
            target_action=action_load_lqr_vmc_controller,
            on_exit=[action_launch_joy],
        )
    )

    return launch.LaunchDescription([
        action_declare_arg_mode_path, 
        action_robot_state_publisher,
        action_launch_gazebo,
        action_gazebo_start,
        delay_joint_state_broadcaster_spawner_after_gazebo_spawn_robot,
        delay_lqr_vmc_controller_spawner_after_joint_state_broadcaster,
        delay_action_launch_joy_after_lqr_vmc_controller_spawner
    ])